// lsiten_key.cpp
# include <ros/ros.h>
# include <iostream>
# include <unistd.h>
# include <termios.h>
#include "ti5arm/ti5_key_cmd.h"
#include <map>
#include <stdlib.h>
#include <string>

char getch()
{

    char buf = 0;
    struct termios old = {0};
    if(tcgetattr(0,&old) < 0)//获取当前终端设置
    {
        perror("tcgetattr()");
    }
    old.c_lflag &= ~ICANON;//关闭标准模式
    old.c_lflag &= ~ECHO;//关闭终端回显
    old.c_cc[VMIN] = 1;//设置非标准模式最小返数据
    old.c_cc[VTIME] = 0;//非标准模式返回超时时间
    if(tcsetattr(0, TCSANOW, &old) < 0)//应用终端设置
    { perror("tcgetattr() ICANON"); }
    if(read(0, &buf, 1) < 0)
    { perror("read()"); }
    old.c_lflag |= ICANON;//打开标准模式
    old.c_lflag |= ECHO;//开启终端回显
    if(tcsetattr(0,TCSADRAIN, &old) < 0)//应用终端设置
    { perror("tcgetattr() ~ICANON"); }
    return buf;

}

class LISTEN_KEY
{
private:
    struct termios old = {0};
    ros::NodeHandle nh;
    
public:
    double motor_add;
    ros::Publisher keyCmdPub;
    LISTEN_KEY(double);
    ~LISTEN_KEY();
    void close_term_std();
    void open_term_std();
    char get_char();
};

LISTEN_KEY::LISTEN_KEY(double motor_add)
{
    this->motor_add = motor_add;
    std::cout<<"start listen key...\n";

    std::cout<<"r,t,y,u,i,o分别对应6个轴的正转；f,g,h,j,k,l分别对应6个轴的反转\n按q退出\n";

    this->keyCmdPub = nh.advertise<ti5arm::ti5_key_cmd>("ti5_key_cmd",10);
}

LISTEN_KEY::~LISTEN_KEY()
{
    std::cout<<"close listen_key...\n";
}

void LISTEN_KEY::close_term_std()
{

    struct termios old = {0};
    if(tcgetattr(0,&old) < 0)//获取当前终端设置
    {
        perror("tcgetattr()");
    }
    old.c_lflag &= ~ICANON;//关闭标准模式
    old.c_lflag &= ~ECHO;//关闭终端回显
    old.c_cc[VMIN] = 1;//设置非标准模式最小返数据
    old.c_cc[VTIME] = 0;//非标准模式返回超时时间
    if(tcsetattr(0, TCSANOW, &old) < 0)//应用终端设置
    { perror("tcgetattr() ICANON"); }

}

void LISTEN_KEY::open_term_std()
{
    struct termios old = {0};
    if(tcgetattr(0,&old) < 0)//获取当前终端设置
    {
        perror("tcgetattr()");
    }
    old.c_lflag |= ICANON;//打开标准模式
    old.c_lflag |= ECHO;//开启终端回显
    if(tcsetattr(0,TCSADRAIN, &old) < 0)//应用终端设置
    { perror("tcgetattr() ~ICANON"); }
}

char LISTEN_KEY::get_char()
{
    char ch;
    if(read(0, &ch, 1) < 0)
    { perror("read()"); }
    return ch;
}

void do_listen()
{
    LISTEN_KEY lk(0.05);
    ros::Rate loop_rate_slow(2);//2Hz
    loop_rate_slow.sleep();
    loop_rate_slow.sleep();

    char ch;
    bool listen_sta = 1;
    std::map <char,int> mp = {{'r',1},{'f',-1},{'R',1},{'F',-1},
                              {'t',2},{'g',-2},{'T',2},{'G',-2},
                              {'y',3},{'h',-3},{'Y',3},{'H',-3},
                              {'u',4},{'j',-4},{'U',4},{'J',-4},
                              {'i',5},{'k',-5},{'I',5},{'K',-5},
                              {'o',6},{'l',-6},{'O',6},{'L',-6}};

    lk.close_term_std();

    double s_time = ros::Time::now().toSec();
    double e_time = s_time;
    ti5arm::ti5_key_cmd keyCmd;
    double mstat[6];

    while (listen_sta)
    {

        mstat[0] = 0;
        mstat[1] = 0;
        mstat[2] = 0;
        mstat[3] = 0;
        mstat[4] = 0;
        mstat[5] = 0;

        ch=lk.get_char();
        // printf("mstat[mp[ch]], mp[ch]: %f, %d\n",mstat[mp[ch]], mp[ch]);
        if (ch=='q'|| ch=='Q')
        {
            listen_sta = 0;
            // std::cout<< "key pressed:"<< ch << "ASCII: "<< (int)ch<<std::endl;
            break;
        }
        else if(abs(mp[ch]) <= 6 &&  abs(mp[ch]) >=1 )
        {   
            
            mstat[abs(mp[ch])-1] += lk.motor_add * mp[ch] / abs(mp[ch]);
            keyCmd.joint1 = mstat[0];
            keyCmd.joint2 = mstat[1];
            keyCmd.joint3 = mstat[2];
            keyCmd.joint4 = mstat[3];
            keyCmd.joint5 = mstat[4];
            keyCmd.joint6 = mstat[5];

            lk.keyCmdPub.publish(keyCmd);
            // printf("msta: %f,%f,%f,%f,%f,%f\n",mstat[0],mstat[1],
            //                                     mstat[2],mstat[3],
            //                                     mstat[4],mstat[5]);
        }
        // std::cout<< "key pressed:"<< ch << ",  ASCII: "<< (int)ch<<std::endl;

        e_time = ros::Time::now().toSec();

        // std::cout<<"fram_rate:"<<1.0/(e_time-s_time)<<std::endl;
        s_time = e_time;
    }


    lk.open_term_std();

}

int main(int argc,  char** argv)
{
    ros::init(argc, argv, "ti5arm_key_cmd_c_node");
    
    do_listen();

    return 0;
}


